using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using Blimpbots;
using Blimpbots.Recognition;
using System.Xml;
using SvgNet;
using SvgNet.SvgElements;

namespace Blimpbots.Control {
    public class Line2D {
        public Point2D a;
        public Point2D b;
        public Line2D(Point2D _a, Point2D _b, string _color) {
            a = _a;
            b = _b;
            color = _color;
        }

        public double intersectsAtAngle(Line2D other, out double intersectDistance) {
            // from usenet: http://groups.google.com/group/microsoft.public.dotnet.framework.drawing/browse_thread/thread/8022e2608700f121/fce4e9611ec572b3%23fce4e9611ec572b3
            // Based on the 2d line intersection method from "comp.graphics.algorithms Frequently Asked Questions"

            /*
                    (Ay-Cy)(Dx-Cx)-(Ax-Cx)(Dy-Cy)
                r = -----------------------------  (eqn 1)
                    (Bx-Ax)(Dy-Cy)-(By-Ay)(Dx-Cx)
             */

            double q = (a.y - other.a.y) * (other.b.x - other.a.x)
                     - (a.x - other.a.x) * (other.b.y - other.a.y);
            double d = (b.x - a.x) * (other.b.y - other.a.y)
                     - (b.y - a.y) * (other.b.x - other.a.x);

            if (d == 0) // parallel lines so no intersection anywhere in space (in curved space, maybe, but not here in Euclidian space.)
            {
                intersectDistance = -1;
                return 0;
            }

            double r = q / d;

            /*
                    (Ay-Cy)(Bx-Ax)-(Ax-Cx)(By-Ay)
                s = -----------------------------  (eqn 2)
                    (Bx-Ax)(Dy-Cy)-(By-Ay)(Dx-Cx)
            */

            q = (a.y - other.a.y) * (b.x - a.x) -
                (a.x - other.a.x) * (b.y - a.y);
            double s = q / d;

            /*
                    If r>1, P is located on extension of AB
                    If r<0, P is located on extension of BA
                    If s>1, P is located on extension of CD
                    If s<0, P is located on extension of DC

                    The above basically checks if the intersection is located at an extrapolated
                    point outside of the line segments. To ensure the intersection is only within
                    the line segments then the above must all be false, ie r between 0 and 1
                    and s between 0 and 1.
            */

            if (r < 0 || r > 1 || s < 0 || s > 1) {
                intersectDistance = -1;
                return 0;
            }

            /*
                Px=Ax+r(Bx-Ax)
                Py=Ay+r(By-Ay)
            */
            Point2D intersectPoint;
            intersectPoint.x = a.x + r * (b.x - a.x);
            intersectPoint.y = a.y + r * (b.y - a.y);
           
            intersectDistance = a.Distance(intersectPoint);

            // end code from usenet
            // we know the lines intersect, find the angle for this line to turn to avoid the line

            return smallestAngleTo(other);
        }
          
        public double smallestAngleTo(Line2D other) {
            // assume point A is at the front of the blimp, so rotate B to make this line parallel
            // (it actually doesn't make a difference which is A and B)

            double thisDx = b.x - a.x;
            double thisDy = b.y - a.y;

            double otherDx = other.b.x - other.a.x;
            double otherDy = other.b.y - other.a.y;

            double thisAngle = Math.Atan2(thisDy, thisDx);

            double otherAngle = Math.Atan2(otherDy, otherDx);

            double angle1 = normalizeAngle(otherAngle - thisAngle);
            double angle2 = normalizeAngle(angle1 - Math.PI);

            if (Math.Abs(angle1) < Math.Abs(angle2)) {
                return angle1;
            }
            else {
                return angle2;
            }
        }

        public double angleTo(Line2D other) {
            // possibly wrong! double check if using, fix other reference

            double thisDx = b.x - a.x;
            double thisDy = b.y - a.y;

            double otherDx = other.b.x - other.a.x;
            double otherDy = other.b.y - other.a.y;

            double thisAngle = Math.Atan2(thisDy, thisDx);

            double otherAngle = Math.Atan2(otherDy, otherDx);

            double angle = normalizeAngle(otherAngle - thisAngle);

            return angle;
          
        }

        public static double toDegrees(double radians) {
            return ((radians / (2 * Math.PI)) * 360);
        }
        
        public static double normalizeAngle(double orientation) {
            while (orientation > Math.PI) {
                orientation -= 2 * Math.PI;
            }

            while (orientation <= -1 * Math.PI) {
                orientation += 2 * Math.PI;
            }

            return orientation;
        }
        public string color;
    }

    class BlimpShape {
        public List<Line2D> edges;
        public List<Line2D> feelers;
        public string name;
        public Line2D intentionVector;

        public bool lastOob;
        public bool lastWall;
        public double orientationSetPoint;
        public double lastDesiredTurn;
    }
    
    public class PolygonWorld {
        private List<Line2D> worldLines;
        private double height;
        private List<BlimpShape> blimps;

        private Point2D roomCenter;

        private double blimpWidth, blimpLength;

        public double xMin, xMax, yMin, yMax;

        public List<Line2D> getWorldLines() {
            return worldLines;
        }

        public List<Line2D> getBlimpLines() {
            List<Line2D> bl = new List<Line2D>();

            foreach (BlimpShape bs in blimps) {
                bl.AddRange(bs.edges);
                bl.AddRange(bs.feelers);
                bl.Add(bs.intentionVector);
            }

            return bl;
        }

        public PolygonWorld(string filename, double _blimpWidth, double _blimpLength, Point2D _roomCenter) {
            worldLines = new List<Line2D>();
            blimps = new List<BlimpShape>();
            height = -1;

            blimpWidth = _blimpWidth;
            blimpLength = _blimpLength;

            roomCenter = _roomCenter;

            xMin = 999999999999999;
            xMax = -999999999999999;
            yMin = 999999999999999;
            yMax = -999999999999999;

            XmlDocument doc = new System.Xml.XmlDocument();
            try {
                doc.Load(filename);
            }
            catch (Exception ex) {
                string s = "Exception: " + ex.Message;
                s += "\r\n\r\nIf this exception mentions an invalid 'x' character, it may well be that a (presumed) bug in the XML libraries is preventing the specified DTD from being parsed properly.  Removing the DTD from the document's DOCTYPE tag may help.";
                Console.WriteLine(s);
                return;
            }
            SvgElement top;

            top = SvgFactory.LoadFromXML(doc, null);
            parseSvgTree(top, " ");
        }

        private void parseSvgTree(SvgElement el, string prefix) {
            // get anything that is a path, and add its segments to the set of lines in the world

            if (el.GetType() == typeof(SvgSvgElement)) {
                // top element
                height = ((SvgSvgElement)el).Height.Value;
            }

            else if (el.GetType() == typeof(SvgPathElement)) {
                if (height == -1) {
                    Console.WriteLine("ERROR: no svg root!");
                    return;
                }

                SvgPathElement spe = (SvgPathElement)el;
                //Console.WriteLine("path: " + spe.D.ToString());

                Point2D prevPoint = new Point2D(0, 0);
                bool ppValid = false;

                for (int i = 0; i < spe.D.Count; i++) {
                    float[] fpoint = spe.D[i].Data;
                    if (fpoint.Length == 2) {
                        // need to reverse the y-coordinate, assuming the SCG editor represents
                        // coordinates starting from bottom-left
                        Point2D point = new Point2D(10 * fpoint[0], 10 * (height - fpoint[1])); // HACK

                        if (ppValid) {
                            worldLines.Add(new Line2D(prevPoint, point, "White"));
                            Console.WriteLine("new seg: " + prevPoint.ToString() + " -> " + point.ToString());
                        }
                        ppValid = true;
                        prevPoint = point;
                        if (point.x < xMin) {
                            xMin = point.x;
                        }
                        if (point.x > xMax) {
                            xMax = point.x;
                        }
                        if (point.y < yMin) {
                            yMin = point.y;
                        }
                        if (point.y > yMax) {
                            yMax = point.y;
                        }
                    }
                    else {
                        // empty point at the end
                    }
                }
            }
            else {
                Console.WriteLine("type: " + el.GetType().ToString());
            }
            ArrayList children = el.Children;
            IEnumerator e = children.GetEnumerator();
            e.Reset();
            while (e.MoveNext()) {
                SvgElement child = (SvgElement)e.Current;
             //   Console.WriteLine(prefix + child.ToString());
                parseSvgTree(child, prefix + " ");
            }
        }

        private BlimpShape getBlimp(String name) {
            foreach (BlimpShape bd in blimps) {
                if (bd.name == name) {
                    return bd;
                }
            }

            return null;
        }

        public double updateBlimp(string blimpName, Point2D position, double orientation, double rotationalVelocity,
            out bool oob, out double oobUrgency) {

            orientation = TwoDimBlimpController.normalizeOrientation(orientation);   

            BlimpShape blimp = getBlimp(blimpName);
            // remove blimp from set of blimps
            // calc the new orientation of the blimp
            // calc the bounding box of the blimp
            // insert blimp in list

            double lastDesiredTurn;
            double orientationSetPoint;
            bool lastOob;
            bool lastWall;
            if (blimp != null)
            {
                // remove the blimp shape from the list
                blimps.Remove(blimp);

                lastDesiredTurn = blimp.lastDesiredTurn;
                orientationSetPoint = blimp.orientationSetPoint;
                lastOob = blimp.lastOob;
                lastWall = blimp.lastWall;
            }
            else
            {
                lastDesiredTurn = 0;
                orientationSetPoint = orientation;
                lastOob = true;
                lastWall = false;
            }

            oobUrgency = 0;

            bool wall = false;

            double feelerLength =  blimpLength;
            double farFeelerDistance = blimpWidth/2.0;

            double halfW = blimpWidth / 2.0;
            double halfL = blimpLength / 2.0;

            // zero-orientation is facing +x

            Point2D backLeft = rotateAboutPoint(position, orientation, new Point2D(position.x - halfL, position.y + halfW));
            Point2D frontLeft = rotateAboutPoint(position, orientation, new Point2D(position.x + halfL, position.y + halfW));
            Point2D backRight = rotateAboutPoint(position, orientation, new Point2D(position.x - halfL, position.y - halfW));
            Point2D frontRight = rotateAboutPoint(position, orientation, new Point2D(position.x + halfL, position.y - halfW));

            Point2D frontCenter = rotateAboutPoint(position, orientation, new Point2D(position.x + halfL, position.y));
            Point2D backCenter = rotateAboutPoint(position, orientation, new Point2D(position.x - halfL, position.y));
            

            // anchors for far feelers 
            Point2D farLeft = rotateAboutPoint(position, orientation, new Point2D(position.x + halfL, position.y + halfW + farFeelerDistance));
            Point2D farRight = rotateAboutPoint(position, orientation, new Point2D(position.x + halfL, position.y - halfW - farFeelerDistance));
            Point2D left = rotateAboutPoint(position, orientation, new Point2D(position.x, position.y + halfW));
            Point2D right = rotateAboutPoint(position, orientation, new Point2D(position.x, position.y - halfW));
            

            Point2D farLeftFeeler = new Point2D(position.x + halfL + feelerLength, position.y + halfW + farFeelerDistance);
            Point2D frontLeftFeeler = new Point2D(position.x + halfL + feelerLength, position.y + halfW);
            Point2D frontRightFeeler = new Point2D(position.x + halfL + feelerLength, position.y - halfW);
            Point2D farRightFeeler = new Point2D(position.x + halfL + feelerLength, position.y - halfW - farFeelerDistance);
            Point2D centerFeeler = new Point2D(position.x + halfL + feelerLength, position.y);

            farLeftFeeler = rotateAboutPoint(position, orientation, farLeftFeeler);
            frontLeftFeeler = rotateAboutPoint(position, orientation, frontLeftFeeler);
            frontRightFeeler = rotateAboutPoint(position, orientation, frontRightFeeler);
            farRightFeeler = rotateAboutPoint(position, orientation, farRightFeeler);
            centerFeeler = rotateAboutPoint(position, orientation, centerFeeler);
            
            /*
            farLeftFeeler = rotateAboutPoint(farLeft, feelerAngle, farLeftFeeler);
            frontLeftFeeler = rotateAboutPoint(frontLeft, feelerAngle, frontLeftFeeler);
            frontRightFeeler = rotateAboutPoint(frontRight, -1*feelerAngle, frontRightFeeler);
            farRightFeeler = rotateAboutPoint(farRight, -1*feelerAngle, farRightFeeler);
            */
            int numFeelers = 5;
            Line2D[] feelers = new Line2D[numFeelers];
            double[] feelerDists = new double[numFeelers];
            double[] feelerAngles = new double[numFeelers];
            feelers[0] = new Line2D(frontLeft, frontLeftFeeler, "Yellow");
            feelers[1] = new Line2D(frontRight, frontRightFeeler, "Yellow");
            feelers[2] = new Line2D(farLeft, farLeftFeeler, "Yellow");
            feelers[3] = new Line2D(farRight, farRightFeeler, "Yellow");
            feelers[4] = new Line2D(frontCenter, centerFeeler, "Yellow");
            feelerDists[0] = feelerLength;
            feelerDists[1] = feelerLength;
            feelerDists[2] = feelerLength;
            feelerDists[3] = feelerLength;
            feelerDists[4] = feelerLength;
            feelerAngles[0] = 0; // MUST be 0, or fix the OOB angle calculation, which assumes this is the vector of the blimp
            feelerAngles[1] = 0;
            feelerAngles[2] = 0;
            feelerAngles[3] = 0;
            feelerAngles[4] = 0;

            Line2D oobCenterFeeler = new Line2D(position, roomCenter, "Magenta");
            Line2D oobFrontFeeler = new Line2D(frontCenter, roomCenter, "Magenta");
            Line2D oobBackFeeler = new Line2D(backCenter, roomCenter, "Magenta");
            
            oob = false;
            double oobAngle = 0;
            double maxOobDist = 0;

            foreach (Line2D line in worldLines) {
                // left = pos, right = neg
                for (int i = 0; i < numFeelers; i++) {
                    double dist;
                    double angle = feelers[i].intersectsAtAngle(line, out dist);
                    if (angle != 0 && dist <= feelerDists[i]) {
                        // intersection (don't care about angle-0 intersections)
                        feelerAngles[i] = angle;
                        feelerDists[i] = dist;
                    }
                }
                // we don't care about the angle, this is then just basically an or
                double oobDist;
                oobAngle = Math.Abs(oobCenterFeeler.intersectsAtAngle(line, out oobDist));
                maxOobDist = max(oobDist, maxOobDist);

                oobAngle += Math.Abs(oobFrontFeeler.intersectsAtAngle(line, out oobDist));
                maxOobDist = max(oobDist, maxOobDist);

                oobAngle += Math.Abs(oobBackFeeler.intersectsAtAngle(line, out oobDist));
                maxOobDist = max(oobDist, maxOobDist);

                if (oobAngle != 0) {
                    oob = true;
                }
            }

            foreach (BlimpShape bs in blimps) {
                foreach (Line2D line in bs.edges) {
                    for (int i = 0; i < numFeelers; i++) {
                        double dist;
                        double angle = feelers[i].intersectsAtAngle(line, out dist);
                        if (angle != 0 && dist <= feelerDists[i]) {
                            // intersection (don't care about angle-0 intersections)
                            // only want to save the closest collision to the blimp
                            feelerAngles[i] = angle;
                            feelerDists[i] = dist;
                        }
                    }
                }
            }

            double turnAngle;
            double minDist = feelerLength;
            // we now have the turn angles for the closest collision to the blimp for each feeler
            if (!oob) {
                int leftVotes = 0;
                int rightVotes = 0;
                double maxLeftAngle = 0;
                double maxRightAngle = 0;

                for (int i = 0; i < numFeelers; i++) {

                    Console.WriteLine("feeler " + i + ": " + feelerAngles[i]);
                    if (feelerDists[i] < minDist) {
                        minDist = feelerDists[i];
                    }
                    if (feelerAngles[i] < 0) {
                        maxRightAngle = max(maxRightAngle, -1 * feelerAngles[i]);
                        rightVotes++;

                    }
                    else if (feelerAngles[i] > 0) {
                        maxLeftAngle = max(maxLeftAngle, feelerAngles[i]);
                        leftVotes++;
                    }
                }

                Console.WriteLine("votes: r " + rightVotes + " l " + leftVotes);

                // disagreement over which direction to turn:
                // if we were trying to turn last update, prefer that direction
                // otherwise, if we have a rotational velocity, prefer continuing in that direction


                if (rightVotes == 0 && leftVotes == 0) {
                    turnAngle = 0;
                }
                else if (rightVotes == 0 && leftVotes > 0) {
                    // turn left
                    turnAngle = -1 * maxLeftAngle;
                    wall = true;
                }
                else if (leftVotes == 0 && rightVotes > 0) {
                    // turn right
                    turnAngle = maxRightAngle;
                    wall = true;
                }
                else {
                    // both directions have votes
                    wall = true;
                    if (lastDesiredTurn < 0) {
                        // last direction was left
                        turnAngle = -1 * maxLeftAngle;
                    }
                    else if (lastDesiredTurn > 0) {
                        turnAngle = maxRightAngle;
                    }
                    else {
                        // no last turn
                        if (rotationalVelocity > 0) {
                            turnAngle = -1 * maxLeftAngle;
                        }
                        else if (rotationalVelocity < 0) {
                            turnAngle = maxRightAngle;
                        }
                        else {
                            // no velocity (probably rare)
                            if (maxLeftAngle > maxRightAngle) {
                                turnAngle = -1 * maxLeftAngle;
                            }
                            else {
                                turnAngle = maxRightAngle;
                            }
                        }
                    }
                }
            } // end if not oob
            else {
                turnAngle = -1*feelers[0].angleTo(oobCenterFeeler);
                oobUrgency = maxOobDist / blimpLength;
                if (oobUrgency > 1) oobUrgency = 1;

                Console.WriteLine("out of bounds! urgency " + oobUrgency + "\n");
            }

            bool overrideTurn = false;

            if (!oob && (lastWall || lastOob) && turnAngle > 0 && lastDesiredTurn < 0 && turnAngle > Math.PI / 4) {
                Console.WriteLine("override turn for continuation");
                turnAngle = Math.PI - turnAngle;
                turnAngle *= -1;
                // turn the complement, in the other direction
                overrideTurn = true;
            }

            if (!oob && (lastWall || lastOob) && turnAngle < 0 && lastDesiredTurn > 0 && turnAngle < -1*(Math.PI / 4)) {
                Console.WriteLine("override turn for continuation");
                turnAngle *= -1;
                turnAngle = Math.PI - turnAngle;
                // turn the complement, in the other direction
                overrideTurn = true;
            }

            /*double urgencyFactor = feelerLength - minDist;
            //urgencyFactor -= feelerLength / 2.0; 

            urgencyFactor = urgencyFactor * ((Math.PI / 2.0) / feelerLength);
            // zero dist looks like a PI/2 angle
            // max dist looks like a 0 angle

            */

            if (turnAngle == 0 && (lastWall || lastOob))
            {
                Console.WriteLine("setting straight-line orientation");
                orientationSetPoint = orientation;
            }
            else if (turnAngle == 0)
            {
                Console.WriteLine("no collisions, seeking to orientation set point");
                turnAngle = -1*(orientationSetPoint - orientation);

                turnAngle /= 2.0; // HACK
            }


            Line2D vector = new Line2D(position, frontCenter, "Green");

            if (turnAngle > 0)
            {
               // turnAngle += urgencyFactor;


                if (turnAngle < 0) turnAngle = 0;
                double urgencyDistance = Math.Abs(turnAngle) * (blimpLength / (Math.PI / 2.0)); 
                Console.WriteLine("turning right " + turnAngle);

                Point2D vectorEnd;
                if (!oob) {
                    vectorEnd = rotateAboutPoint(position, orientation, new Point2D(position.x, position.y - halfW - urgencyDistance));
                    vector = new Line2D(right, vectorEnd, "Green");
                }
                else {
                    vectorEnd = rotateAboutPoint(position, orientation-turnAngle, new Point2D(position.x + oobUrgency * blimpLength, position.y));
                    vector = new Line2D(position, vectorEnd, "Red");
                }
            }
            if (turnAngle < 0)
            {
                //turnAngle -= urgencyFactor;


                if (turnAngle > 0) turnAngle = 0;
                double urgencyDistance = Math.Abs(turnAngle) * (blimpLength / (Math.PI / 2.0)); 
                Console.WriteLine("turning left " + turnAngle);

                Point2D vectorEnd;
                if (!oob) {
                    vectorEnd = rotateAboutPoint(position, orientation, new Point2D(position.x, position.y + halfW + urgencyDistance));
                    vector = new Line2D(left, vectorEnd, "Green");
                }
                else {
                    vectorEnd = rotateAboutPoint(position, orientation-turnAngle, new Point2D(position.x + oobUrgency * blimpLength, position.y));
                    vector = new Line2D(position, vectorEnd, "Red");
                }
                
            }

            if (overrideTurn) {
                vector.color = "Orange";
            }
            

            // now, add the blimp bounding box to the struct

            BlimpShape newBS = new BlimpShape();
            newBS.name = blimpName;
            newBS.edges = new List<Line2D>();

            newBS.edges.Add(new Line2D(frontLeft, frontRight, "Cyan"));
            newBS.edges.Add(new Line2D(frontRight, backRight, "Cyan"));
            newBS.edges.Add(new Line2D(backRight, backLeft, "Cyan"));
            newBS.edges.Add(new Line2D(backLeft, frontLeft, "Cyan"));

            newBS.feelers = new List<Line2D>();
            for (int i = 0; i < numFeelers; i++) {
                newBS.feelers.Add(feelers[i]);
            }

            newBS.intentionVector = vector;

            Console.WriteLine("iv: " + vector.a.ToString() + " " + vector.b.ToString());

            newBS.lastOob = oob;
            newBS.lastDesiredTurn = turnAngle;
            newBS.orientationSetPoint = orientationSetPoint;
            newBS.lastWall = wall;

            blimps.Add(newBS);

            return turnAngle;
        
        }
        private double max(double value1, double value2) {
            if (value1 > value2) {
                return value1;
            }
            return value2;
        }

        public static Point2D rotateAboutPoint(Point2D origin, double radians, Point2D point) {
            point.x -= origin.x;
            point.y -= origin.y;

            point = rotatePoint(point, radians);

            point.x += origin.x;
            point.y += origin.y;

            return point;
        }

        private static Point2D rotatePoint(Point2D p, double a) {
            return new Point2D(p.x * Math.Cos(a) - p.y * Math.Sin(a),
                               p.y * Math.Cos(a) + p.x * Math.Sin(a));
        }
    }
}
